package edu.mapi.ir.controllers;

import java.awt.geom.Point2D;
import java.io.File;
import java.util.Vector;

import edu.mapi.ir.planning.AStarSearch;
import edu.mapi.ir.state.EstimatedState;
import edu.mapi.ir.viewer.GridPosition;
import edu.mapi.ir.viewer.Maze;

/**
 * This class is used to test the controllers using only the state estimator (therefore no errors are accounted)
 * @author ZP
 */
public class ControllerSandbox {

	private EstimatedState state = new EstimatedState();
	private Actuation lastActuation = new Actuation(0,0);
	
	
	public void testController(AbstractController controller) {
		
		while (!controller.isComplete()) {
			lastActuation = controller.decide(lastActuation, state);
			state.actuate(lastActuation.leftPower, lastActuation.rightPower);
			System.out.println(state);
		}
	
		StopController stop = new StopController();
		lastActuation = stop.decide(lastActuation, state);
		state.actuate(lastActuation.leftPower, lastActuation.rightPower);
		System.out.println(state);

	}
	
	public static void main(String[] args) throws Exception {
		ControllerSandbox sandbox = new ControllerSandbox();
		Maze maze = Maze.readXML(new File("c:/maze21.xml"), new File("c:/grid.xml"));
		Vector<GridPosition> grid = maze.getGrid();
		
		EstimatedState state = new EstimatedState();
		
		state.setX(grid.firstElement().position.getX());
		state.setY(grid.firstElement().position.getY());
		state.setRotation(Math.toRadians(grid.firstElement().direction));
		
		
		System.out.println("first state is "+state);
		
		AStarSearch search = new AStarSearch(maze);
		
		Vector<Point2D> bestPath = search.getBestPath(grid.firstElement().position, maze.getBeacon().getPosition(), 0);														

		AbstractController[] controllers = new AbstractController[bestPath.size()]; 
			
		GoToController cont = new GoToController(
				grid.firstElement().position.getX(), 
				grid.firstElement().position.getY(),
				bestPath.get(0).getX(),
				bestPath.get(0).getY()
		);
		controllers[0] = cont;
		
		for (int i = 1; i < bestPath.size(); i++) {
			controllers[i] = new GoToController(
					bestPath.get(i-1).getX(),
					bestPath.get(i-1).getY(),
					bestPath.get(i).getX(),
					bestPath.get(i).getY()
			);
		}
		SequenceController controller = new SequenceController();
		controller.setControllers(controllers);
		
		/*
		sandbox.testController(new RotateController(27));
		System.out.println("should be 27");
		sandbox.testController(new RotateController(-27));
		System.out.println("should be -27");
		sandbox.testController(new RotateController(27));
		System.out.println("should be 27");
		*/
						
		//sandbox.testController(new GoToController(0,0,13,-8));
		//sandbox.testController(new GoToController(13,-8,13.23,12.76));
		sandbox.testController(controller);
	}
	
	
}
